% 无电机的关节pid控制
clc;
clear;


%仍使用不含电机的关节，所以关节的控制器也保持原有的
%现在是因为可能需要一次性获得多组数据，所以使用n_group来规定需要几组数据
%确实不用电机，直接pid搞出力矩
%水平初始位置
n_group = 1;
initial_state = [pi/2 0];
a_sj = singleJoint(initial_state);

kp = 2.927
ki = 0.313
kd = 50

simu_period = 1e-5;
simu_step = 5e4;
sample_period = 1e-3;
% res = a_sj.pos_step_control(kp,ki,kd,sample_period,simu_step,simu_period);
%弃用类内

%这个是位置阶跃相应的一系列操作
target_theta = pi/2-1*pi/180
pos_pid = pidControler(a_sj.n_group,kp,ki,kd,target_theta);

state_info = zeros(a_sj.n_group,length(1:simu_step),3);
debug_info = zeros(a_sj.n_group,length(1:simu_step),4);
con_value = 0;
sample_time_count = 0;
for i = 1:simu_step
    state_info(:,i,:) = (a_sj.simu_loop(con_value,simu_period)).';
    sample_time_count = sample_time_count+simu_period;
    if(sample_time_count >= sample_period)
        debug_info(:,i,:) = pos_pid.get_sum_up_con_value(a_sj.state(:,1));
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    end
end


anima = 0;  % 0 for no animation ,just for debug
if(anima == 0)
    group_name = {'g1'};
    title_name = {'theta','omega','acc'};
    for i = 1:3
        figure;
        for j = 1:n_group
            plot(0:simu_period:(simu_step-1)*simu_period,state_info(j,:,i));
            hold on;
        end
%         legend(group_name);
        title(title_name(i));
    end
else
    n_points = simu_step;
    big_per = 100;
    figure;
    pause(8);
    for n=1:big_per:n_points-(big_per-1)
        %绘图区
        plot([0 a_sj.l*sin(state_info(1,n,1))],[0 a_sj.l*cos(state_info(1,n,1))],'b','LineWidth',2); % Pendulum rod
        rectangle('Position',[-0.002,-0.002 ,0.004,0.004],'Curvature',0.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1]);
        set(gca,'Ylim',[-0.01 0.01],'Xlim',[0 a_sj.l+0.01]);
        drawnow;
    end
end